
#define LOG_TAG "GlTest"

#include "Camera.h"

namespace android {


    CameraDevice::CameraDevice(sp<FrameQueue> q) {
        mQueue = q;
    }

    CameraDevice::~CameraDevice() {
    	stopcapturing();
    	uninitdevice();
    	closedevice();
    
    	if(rgb)
    		rgb = NULL;
    	if(ybuf)
    		free(ybuf);        
    	fd = -1;
    }

    void CameraDevice::onFirstRef() {
        run("Camera", PRIORITY_DISPLAY);
    }

    status_t CameraDevice::readyToRun() {
        int idev = 2;
        status_t ret;

		ybuf = (int *)malloc(sizeof(int) * (IMG_WIDTH*IMG_HEIGHT));
        if (ybuf == NULL) {
            return NO_MEMORY;
        }

        ALOGD("%s: v4l2 dev /dev/video%d", __func__, idev);
        opendevice(idev);
		initdevice();

        return NO_ERROR;
    }

    bool CameraDevice::threadLoop() {
        ALOGD("%s: enter loop", __func__);

        startcapturing();
        readframeonce();


        ALOGD("%s: exit loop", __func__);

        // when thread exit
    	stopcapturing();

        return NO_ERROR;
    }

    int CameraDevice::errnoexit(const char *s) {
    	ALOGE("%s error %d, %s", s, errno, strerror(errno));
    	return ERROR_LOCAL;
    }
    
    int CameraDevice::xioctl(int fd, int request, void *arg)
    {
    	int r;
    
    	do {
    		r = ioctl(fd, request, arg);
    	}while (-1 == r && EINTR == errno);
    
    	return r;
    }
    
    int CameraDevice::opendevice(int i)
    {
    	struct stat st;
    
    	sprintf(dev_name, "/dev/video%d", i);
    
    	if (-1 == stat(dev_name, &st)) {
    		ALOGE("Cannot identify '%s': %d, %s", dev_name, errno, strerror(errno));
    		return ERROR_LOCAL;
    	}
    
    	if (!S_ISCHR(st.st_mode)) {
    		ALOGE("%s is no device", dev_name);
    		return ERROR_LOCAL;
    	}
    
    	fd = open(dev_name, O_RDWR | O_NONBLOCK, 0);
    
    	if (-1 == fd) {
    		ALOGE("Cannot open '%s': %d, %s", dev_name, errno, strerror(errno));
    		return ERROR_LOCAL;
    	}
    	return SUCCESS_LOCAL;
    }
    
    int CameraDevice::initdevice(void) {
    	struct v4l2_capability cap;
    	struct v4l2_cropcap cropcap;
    	struct v4l2_crop crop;
    	struct v4l2_format fmt;
    	unsigned int min;
    
    	if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
    		if (EINVAL == errno) {
    			ALOGE("%s is no V4L2 device", dev_name);
    			return ERROR_LOCAL;
    		} else {
    			return errnoexit("VIDIOC_QUERYCAP");
    		}
    	}
    
    	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    		ALOGE("%s is no video capture device", dev_name);
    		return ERROR_LOCAL;
    	}
    
    	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
    		ALOGE("%s does not support streaming i/o", dev_name);
    		return ERROR_LOCAL;
    	}

        {
            fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        	if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt))
        		return errnoexit("VIDIOC_G_FMT");
    
            ALOGD("%s: VIDIOC_G_FMT width = %d", __func__, fmt.fmt.pix.width);
            ALOGD("%s: VIDIOC_G_FMT height = %d", __func__, fmt.fmt.pix.height);
        }
    
    	CLEAR(cropcap);
    
    	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    
    	if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
    		crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    		crop.c = cropcap.defrect;
    
    		if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop)) {
    			switch (errno) {
    			case EINVAL:
    				break;
    			default:
    				break;
    			}
    		}
    	} else {
    	}

        CLEAR(fmt);
    
    	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    
    	fmt.fmt.pix.width = IMG_WIDTH;
    	fmt.fmt.pix.height = IMG_HEIGHT;
    
    	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    	fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
    
    	if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
    		return errnoexit("VIDIOC_S_FMT");
    
    	min = fmt.fmt.pix.width * 2;
    	if (fmt.fmt.pix.bytesperline < min)
    		fmt.fmt.pix.bytesperline = min;
    	min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
    	if (fmt.fmt.pix.sizeimage < min)
    		fmt.fmt.pix.sizeimage = min;
    
    	return initmmap();
    }
    
    int CameraDevice::initmmap(void) {
    	struct v4l2_requestbuffers req;
    
    	CLEAR(req);
    
    	req.count = 4;
    	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    	req.memory = V4L2_MEMORY_MMAP;
    
    	if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
    		if (EINVAL == errno) {
    			ALOGE("%s does not support memory mapping", dev_name);
    			return ERROR_LOCAL;
    		} else {
    			return errnoexit("VIDIOC_REQBUFS");
    		}
    	}
    
    	if (req.count < 2) {
    		ALOGE("Insufficient buffer memory on %s", dev_name);
    		return ERROR_LOCAL;
    	}
    
    	buffers = (struct buffer*)calloc(req.count, sizeof(*buffers));
    
    	if (!buffers) {
    		ALOGE("Out of memory");
    		return ERROR_LOCAL;
    	}
    
    	for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
    		struct v4l2_buffer buf;
    
    		CLEAR(buf);
    
    		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    		buf.memory = V4L2_MEMORY_MMAP;
    		buf.index = n_buffers;
    
    		if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
    			return errnoexit("VIDIOC_QUERYBUF");
    
    		buffers[n_buffers].length = buf.length;
    		buffers[n_buffers].start = mmap(NULL, buf.length,
    				PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
    
    		if (MAP_FAILED == buffers[n_buffers].start)
    			return errnoexit("mmap");
    	}
    
    	return SUCCESS_LOCAL;
    }
    
    int CameraDevice::startcapturing(void) {
    	unsigned int i;
    	enum v4l2_buf_type type;
    
    	for (i = 0; i < n_buffers; ++i) {
    		struct v4l2_buffer buf;
    
    		CLEAR(buf);
    
    		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    		buf.memory = V4L2_MEMORY_MMAP;
    		buf.index = i;
    
    		if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
    			return errnoexit("VIDIOC_QBUF");
    	}
    
    	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    
    	if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
    		return errnoexit("VIDIOC_STREAMON");
    
    	return SUCCESS_LOCAL;
    }
    
    int CameraDevice::readframeonce(void) {
    	for (;;) {
    		fd_set fds;
    		struct timeval tv;
    		int r;
    
    		FD_ZERO(&fds);
    		FD_SET(fd, &fds);
    
    		tv.tv_sec = 2;
    		tv.tv_usec = 0;
    
    		r = select(fd + 1, &fds, NULL, NULL, &tv);
    		if (-1 == r) {
    			if (EINTR == errno)
    				continue;
    
    			return errnoexit("select");
    		}
    		if (0 == r) {
    			ALOGE("select timeout");
    			return ERROR_LOCAL;
    
    		}
    		if (readframe() < 1)
    			break;
    	}
    	return SUCCESS_LOCAL;
    }
    
    void CameraDevice::processimage(const void *p) {
        int idx = -1;
        if (mQueue->dequeueBuffer(&idx, (void **)&rgb) != NO_ERROR) {
            return;
        }

        ALOGD("%s: idx = %d, rgb = %p", __func__, idx, rgb);
        if (idx < 0 || rgb == NULL) {
            ALOGE("%s: idx = %d, rgb = %p", __func__, idx, rgb);
            return;
        }

    	//yuyv422toABGRY((unsigned char *) p);
    	yuyv422toBMP((unsigned char *) p);

        mQueue->queueBuffer(idx);
    }
    
    int CameraDevice::readframe(void)
    {
    	struct v4l2_buffer buf;
    	unsigned int i;
    
    	CLEAR(buf);
    
    	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    	buf.memory = V4L2_MEMORY_MMAP;
    
    	if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf))
    	{
    		switch (errno)
    		{
    		case EAGAIN:
    			return 0;
    		case EIO:
    		default:
    			return errnoexit("VIDIOC_DQBUF");
    		}
    	}
    
    	assert(buf.index < n_buffers);
    	processimage(buffers[buf.index].start);
    	if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
    		return errnoexit("VIDIOC_QBUF");
    
    	return 1;
    }
    
    int CameraDevice::stopcapturing(void) {
    	enum v4l2_buf_type type;
    
    	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    	if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
    		return errnoexit("VIDIOC_STREAMOFF");
    	return SUCCESS_LOCAL;
    }
    
    int CameraDevice::uninitdevice(void) {
    	unsigned int i;
    
    	for (i = 0; i < n_buffers; ++i) {
    		if (-1 == munmap(buffers[i].start, buffers[i].length))
    			return errnoexit("munmap");
    	}
    	free(buffers);
    	return SUCCESS_LOCAL;
    }
    
    int CameraDevice::closedevice(void) {
    	if (-1 == close(fd)) {
    		fd = -1;
    		return errnoexit("close");
    	}
    	fd = -1;
    	return SUCCESS_LOCAL;
    }
    
    void CameraDevice::yuyv422toABGRY(unsigned char *src) {
    
    	int width = 0;
    	int height = 0;
    
    	width = IMG_WIDTH;
    	height = IMG_HEIGHT;
    
    	int frameSize = width * height * 2;
    
    	int i;
    
    	if ((!rgb || !ybuf)) {
    		return;
    	}
    	int *lrgb = NULL;
    	int *lybuf = NULL;
    
        // add  bmp header
        memset(rgb, 0, width*height*4);
        memcpy(rgb, &bfile, sizeof(BITMAPFILEHEADER));
        memcpy((uint8_t *)rgb + sizeof(BITMAPFILEHEADER), &binfo, sizeof(BITMAPINFOHEADER));

    	//lrgb = &rgb[0];
        lrgb = (int *)((uint8_t *)rgb + sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER));
    	lybuf = &ybuf[0];
    
    	if (yuv_tbl_ready == 0) {
    		for (i = 0; i < 256; i++) {
    			y1192_tbl[i] = 1192 * (i - 16);
    			if (y1192_tbl[i] < 0) {
    				y1192_tbl[i] = 0;
    			}
    
    			v1634_tbl[i] = 1634 * (i - 128);
    			v833_tbl[i] = 833 * (i - 128);
    			u400_tbl[i] = 400 * (i - 128);
    			u2066_tbl[i] = 2066 * (i - 128);
    		}
    		yuv_tbl_ready = 1;
    	}
    
    	for (i = 0; i < frameSize; i += 4) {
    		unsigned char y1, y2, u, v;
    		y1 = src[i];
    		u = src[i + 1];
    		y2 = src[i + 2];
    		v = src[i + 3];
    
    		int y1192_1 = y1192_tbl[y1];
    		int r1 = (y1192_1 + v1634_tbl[v]) >> 10;
    		int g1 = (y1192_1 - v833_tbl[v] - u400_tbl[u]) >> 10;
    		int b1 = (y1192_1 + u2066_tbl[u]) >> 10;
    
    		int y1192_2 = y1192_tbl[y2];
    		int r2 = (y1192_2 + v1634_tbl[v]) >> 10;
    		int g2 = (y1192_2 - v833_tbl[v] - u400_tbl[u]) >> 10;
    		int b2 = (y1192_2 + u2066_tbl[u]) >> 10;
    
    		r1 = r1 > 255 ? 255 : r1 < 0 ? 0 : r1;
    		g1 = g1 > 255 ? 255 : g1 < 0 ? 0 : g1;
    		b1 = b1 > 255 ? 255 : b1 < 0 ? 0 : b1;
    		r2 = r2 > 255 ? 255 : r2 < 0 ? 0 : r2;
    		g2 = g2 > 255 ? 255 : g2 < 0 ? 0 : g2;
    		b2 = b2 > 255 ? 255 : b2 < 0 ? 0 : b2;
    
    		*lrgb++ = 0xff000000 | b1 << 16 | g1 << 8 | r1;
    		*lrgb++ = 0xff000000 | b2 << 16 | g2 << 8 | r2;
    		//*lrgb++ = 0xff000000 | r1 << 16 | g1 << 8 | b1;
    		//*lrgb++ = 0xff000000 | r2 << 16 | g2 << 8 | b2;
    
    		if (lybuf != NULL) {
    			*lybuf++ = y1;
    			*lybuf++ = y2;
    		}
    	}

        {
            // write to file
            FILE* fBMP = fopen("/data/a.bmp", "w");
            if (!fBMP) {
                ALOGE("%s: fopen fail", __func__);
                return;
            }
            fwrite(rgb, sizeof(uint8_t), width*height*3+54, fBMP);
            fclose(fBMP);
            exit(255);
        
        }
    }

void CameraDevice::yCbCr422_normalization(struct yuv422_sample *yuv422_samp_in,
			  struct yuv422_sample *yuv422_samp_out)
{
	uint8_t y0 = 0;
	uint8_t cb = 0;
	uint8_t y1 = 0;
	uint8_t cr = 0;

//	if (!(DF_IS_YCbCr422(fmt)))
//		return;

//	switch(fmt & YCbCr422_MASK) {
//	case YCbCr422_Y0CbY1Cr:
		y0 = yuv422_samp_in->b1;
		cb = yuv422_samp_in->b2;
		y1 = yuv422_samp_in->b3;
		cr = yuv422_samp_in->b4;
//		break;
/*	case YCbCr422_CbY1CrY0:
		cb = yuv422_samp_in->b1;
		y1 = yuv422_samp_in->b2;
		cr = yuv422_samp_in->b3;
		y0 = yuv422_samp_in->b4;
		break;
	case YCbCr422_Y1CrY0Cb:
		y1 = yuv422_samp_in->b1;
		cr = yuv422_samp_in->b2;
		y0 = yuv422_samp_in->b3;
		cb = yuv422_samp_in->b4;
		break;
	case YCbCr422_CrY0CbY1:
		cr = yuv422_samp_in->b1;
		y0 = yuv422_samp_in->b2;
		cb = yuv422_samp_in->b3;
		y1 = yuv422_samp_in->b4;
		break;
	case YCbCr422_CrY1CbY0:
		cr = yuv422_samp_in->b1;
		y1 = yuv422_samp_in->b2;
		cb = yuv422_samp_in->b3;
		y0 = yuv422_samp_in->b4;
		break;
	case YCbCr422_Y1CbY0Cr:
		y1 = yuv422_samp_in->b1;
		cb = yuv422_samp_in->b2;
		y0 = yuv422_samp_in->b3;
		cr = yuv422_samp_in->b4;
		break;
	case YCbCr422_CbY0CrY1:
		cb = yuv422_samp_in->b1;
		y0 = yuv422_samp_in->b2;
		cr = yuv422_samp_in->b3;
		y1 = yuv422_samp_in->b4;
		break;
	case YCbCr422_Y0CrY1Cb:
		y0 = yuv422_samp_in->b1;
		cr = yuv422_samp_in->b2;
		y1 = yuv422_samp_in->b3;
		cb = yuv422_samp_in->b4;
		break;
	default:
		;
	}
*/
	yuv422_samp_out->b1 = y0;
	yuv422_samp_out->b2 = cb;
	yuv422_samp_out->b3 = y1;
	yuv422_samp_out->b4 = cr;
}

    void CameraDevice::yuyv422toBMP(uint8_t *frame) {
        int width = IMG_WIDTH;
        int height = IMG_HEIGHT;
        int img_size = IMG_WIDTH * IMG_HEIGHT;
		uint8_t* y = frame;
		uint8_t* cb = frame + img_size;
		uint8_t* cr = frame + img_size + img_size / 2;
	    uint8_t y0, y1, u, v;
	    struct yuv422_sample yuv422_samp;
    	struct yuv422_sample *yuv422_pixel =  NULL;
        int colorR, colorG, colorB;

    	if ((!rgb || !ybuf)) {
    		return;
    	}
    	int *lrgb = NULL;
    	int *lybuf = NULL;
    
        // add  bmp header
        memset(rgb, 0, width*height*4);
        memcpy(rgb, &bfile, sizeof(BITMAPFILEHEADER));
        memcpy((uint8_t *)rgb + sizeof(BITMAPFILEHEADER), &binfo, sizeof(BITMAPINFOHEADER));

	    struct bmp_bgr *bgr = (struct bmp_bgr *)((uint8_t *)rgb + sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER));
 
    	for (int line = 0; line < height; line += 1) {
            yuv422_pixel = (struct yuv422_sample *)(frame + line * width * 2);

		    for (int col = 0; col < width; col += 2) {
				yCbCr422_normalization(yuv422_pixel, &yuv422_samp);
				y0 = yuv422_samp.b1;
				u = yuv422_samp.b2;
				y1 = yuv422_samp.b3;
				v = yuv422_samp.b4;

				yuv422_pixel ++;


                //convert from YUV domain to RGB domain
                colorB = y0 + ((443 * (u - 128)) >> 8);
                colorB = (colorB < 0) ? 0 : ((colorB > 255 ) ? 255 : colorB);
                
                colorG = y0 -
                	((179 * (v - 128) +
                	  86 * (u - 128)) >> 8);
                
                colorG = (colorG < 0) ? 0 : ((colorG > 255 ) ? 255 : colorG);
                
                colorR = y0 + ((351 * (v - 128)) >> 8);
                colorR = (colorR < 0) ? 0 : ((colorR > 255 ) ? 255 : colorR);
                
                //Windows BitMap BGR
                bgr->blue = colorB;
                bgr->green = colorG;
                bgr->red = colorR;
                bgr++;
                
                colorB = y1 + ((443 * (u - 128)) >> 8);
                colorB = (colorB < 0) ? 0 : ((colorB > 255 ) ? 255 : colorB);
                
                colorG = y1 -
                	((179 * (v - 128) +
                	  86 * (u - 128)) >> 8);
                
                colorG = (colorG < 0) ? 0 : ((colorG > 255 ) ? 255 : colorG);
                
                colorR = y1 + ((351 * (v - 128)) >> 8);
                colorR = (colorR < 0) ? 0 : ((colorR > 255 ) ? 255 : colorR);
                
                //Windows BitMap BGR
                bgr->blue = colorB;
                bgr->green = colorG;
                bgr->red = colorR;
                bgr++;
            }

        }
#if 0        
         {
            // write to file
            FILE* fBMP = fopen("/data/b.bmp", "w");
            if (!fBMP) {
                ALOGE("%s: fopen fail", __func__);
                return;
            }
            fwrite(rgb, sizeof(uint8_t), width*height*3+54, fBMP);
            fclose(fBMP);
            exit(255);
        
        }
#endif
    }
    
}   // namespace
